#include "Ctrl.h"


/// @brief 构造函数
PCBmachine::PCBmachine()
{
    this->PCBmachine_ok = true;
    
    SetFlage(IMG_SIG, false);  // 拍照标志
    SetFlage(M1_SIG, true); // 一号电机复位标志
    SetFlage(M2_SIG, true); // 二号电机复位标志
    SetFlage(STEP4_SIG, false);  // 升降台复位标志
    SetFlage(PCB_ON_AssemblyLine, false); // 有无pcb在2号位置标志
    SetFlage(PCB_ON_Platform, false); // 有无pcb在升降台位置标志

    /*插入指令回调函数*/
    this->insertCmd(CMD_TEST, std::bind(&PCBmachine::CmdTest, this, std::placeholders::_1));
    this->insertCmd(CMD_MOTOR_GETPOS, std::bind(&PCBmachine::CmdPose, this, std::placeholders::_1));
    this->insertCmd(CMD_MOTOR_ERR, std::bind(&PCBmachine::CmdErr, this, std::placeholders::_1));

    /*初始化发送和接收线程*/
    this->begin_sr_thread();
    
    /*推出第一块PCB*/
    Step1();
}

/// @brief 析构函数
PCBmachine::~PCBmachine()
{
    /* 尝试复位电机 */
    this->CtrlM3_4(0);
    wait_for_motor_movement(3, 0);
    
    this->PCBmachine_ok = false;
}

/// @brief 将PCB放入指定位置中
/// @param x 可选 1 、0，表示左右
/// @param y 
void PCBmachine::place(int x, int y)
{
    std::thread s1(&PCBmachine::Step1, this);
    std::thread s2(&PCBmachine::Step2, this);
    std::thread s3(&PCBmachine::Step3, this, x, y);

    s1.detach();
    s2.detach();
    s3.detach();
    
}


/// @brief 驱使1号电机将pcb推至2号电机处
void PCBmachine::Step1()
{
    std::lock_guard<std::mutex> lock(mutex_Step[0]);
    /*等待2号电机复位*/
    while (!this->GetFlage(M2_SIG));
    /*等待2号电机处理完pcb*/
    while (this->GetFlage(PCB_ON_AssemblyLine));

    /*表示1号电机正在工作*/
    SetFlage(M1_SIG, false);

    /*驱动1号电机推动pcb并复位*/
    DBG("Step1 1号电机推入PCB\n");
    this->CtrlM1(186000); // 190000
    this->wait_for_motor_movement(1, 186800, true, 400);
    delay_ms(100);

    SetFlage(PCB_ON_AssemblyLine, true);   // 表示有pcb在2号电机处
    SetFlage(IMG_SIG, true); // 可以开始检测pcb
    
    this->CtrlM1(0); DBG("Step1 1号电机复位\n");
    this->wait_for_motor_movement(1, 0, true, 400);
    delay_ms(100);


    SetFlage(M1_SIG, true);  // 表示1号电机复位


}

/// @brief 驱使2号电机将pcb推至平台
void PCBmachine::Step2()
{
    std::lock_guard<std::mutex> lock(mutex_Step[1]);

    /*等待新的pcb从1号电机推出*/
    while (!this->GetFlage(PCB_ON_AssemblyLine));
    /*等待升降台复位*/
    while (!this->GetFlage(STEP4_SIG));

    SetFlage(IMG_SIG, false); // 不可检测pcb

    /*表示2号电机正在工作*/
    SetFlage(M2_SIG, false);

    /*驱动2号电机推动pcb并复位*/
    DBG("Step2 2号电机推动PCB至升降台\n");
    CtrlM2(100000);
    this->wait_for_motor_movement(2, 100000, true, 400);
    delay_ms(400);
    DBG("Step2 2号电机复位\n");
    CtrlM2(0);
    this->wait_for_motor_movement(2, 0, true, 400);
    delay_ms(400);
    /*!!!!!!!!待完成!!!!!!!!*/

    SetFlage(M2_SIG, true);                 // 2号电机复位
    SetFlage(PCB_ON_AssemblyLine, false);   // 无pcb在2号位置
    SetFlage(PCB_ON_Platform, true);        // 有pcb在升降台位置
}

/// @brief 驱动升降台和56号电机推动pcb前往指定位置,并复位升降台
/// @param x 左边:0 右边:1
/// @param y 
void PCBmachine::Step3(int x, int y)
{
    static uint32_t STEP_M5 = 116800; // M5最大步进
    static uint32_t STEP_M6 = 118400; // M6最大步进
    static uint32_t STEP_Y0_0 = 20000; // 第一个格子对应的高度的步进 右边
    static uint32_t STEP_DY_0 = 25000; // 每个格子的高度间隔 右边 (3200*8)=25600
    static uint32_t STEP_Y0_1 = 23000; // 第一个格子对应的高度的步进 左边
    static uint32_t STEP_DY_1 = 25000; // 每个格子的高度间隔 右边
    uint32_t STEP_Y = 0; 
    std::lock_guard<std::mutex> lock(mutex_Step[2]);

    SetFlage(STEP4_SIG, false); // 表示升降台正在处理滑块

    /*根据x坐标处理两个滑块*/
    if(x == 0)
    {
        STEP_Y  = STEP_Y0_0 + y * STEP_DY_0;
        /*两个滑块都复位到右边, 同时控制升降台归零*/
        this->CtrlM5(STEP_M5);
        this->CtrlM6(0);
        this->reset_platform(true);
        this->wait_for_motor_movement(5, STEP_M5, true, 200);
        this->wait_for_motor_movement(6, 0, true, 200);
        delay_ms(400);

    }else if (x == 1)
    {
        STEP_Y = STEP_Y0_1 + y * STEP_DY_1;
        /*两个滑块都复位到左边, 同时控制升降台归零*/
        this->CtrlM6(STEP_M6);
        this->CtrlM5(0);
        this->reset_platform(true);
        this->wait_for_motor_movement(6, STEP_M6, true, 200);
        this->wait_for_motor_movement(5, 0, true, 200);
        delay_ms(400);

    }else
    {
        std::cout<<"步骤三: x坐标错误"<<std::endl;
        return;
    }

    SetFlage(STEP4_SIG, true); // 表示升降台复位
    
    /*等待新的pcb推向升降台*/
    while (!this->GetFlage(PCB_ON_Platform));

    SetFlage(STEP4_SIG, false); // 表示升降台正在将板子推到指定位置

    /*驱动升降台和56号电机推动pcb前往指定位置,并复位升降台*/
    DBG("Step3升降台启动\n");
    this->CtrlM3_4(STEP_Y);// 第一个格子处步进为20000
    this->wait_for_motor_movement(3, STEP_Y, true, 200);

    /*将板子推向格子*/
    if (x == 0)
    {
        WAIT_INPUT_YES();
        this->CtrlM5(0);
        this->CtrlM6(118400);
        this->wait_for_motor_movement(5, 0, true, 200);
        this->wait_for_motor_movement(6, 118400, true, 200);
    }
    else 
    {
        WAIT_INPUT_YES();
        this->CtrlM6(0);
        delay_ms(1000);
        this->CtrlM5(116800);
        this->wait_for_motor_movement(6, 0, true, 200);
        this->wait_for_motor_movement(5, 116800, true, 200);
    }

    /*复位滑块, 复位平台*/
    this->reset_platform(true);

    DBG("Step3升降台复位\n");
    /*!!!!!!!!待完成!!!!!!!!*/
    
    SetFlage(PCB_ON_Platform, false); // 无pcb在升降台位置
}

/// @brief 设置对应标志
/// @param id 对应标志, 具体定义在PCBFlage枚举类型中
/// @param state true 或 false
/// @return state
bool PCBmachine::SetFlage(PCBFlage id, bool state)
{
    std::lock_guard<std::mutex> lock(mutex_Flage);
    Flage[id] = state;
    return Flage[id];
}

/// @brief 获取对应标志
/// @param id 对应标志, 具体定义在PCBFlage枚举类型中
/// @param state 
/// @return 
bool PCBmachine::GetFlage(PCBFlage id, bool& state)
{
    std::lock_guard<std::mutex> lock(mutex_Flage);
    state = Flage[id];
    return Flage[id];
}

/// @brief 获取对应标志
/// @param id 对应标志, 具体定义在PCBFlage枚举类型中
/// @return 
bool PCBmachine::GetFlage(PCBFlage id)
{
    std::lock_guard<std::mutex> lock(mutex_Flage);
    return Flage[id];
}

/// @brief 获取拍照信号,非阻塞
/// @return true表示可以拍照检测
bool PCBmachine::GetImgSignal()
{
    return this->GetFlage(IMG_SIG);
}

/// @brief 将u32 count存储在u8 arr[4]中 大端模式
/// @param arr 长度为4的u8数组
/// @param count 
void PCBmachine::U32_TO_U8ARR(uint8_t arr[], uint32_t count)
{
    for (int i=0; i<4; i++)
    {
        arr[i] = (count >> (8 * (3 - i))) & 0xFF;
    }
}

uint32_t PCBmachine::angle_to_step(double angle)
{
    uint32_t step = 0;
    
    step = (angle / 360.0) * 3200;

    return step;
}

double PCBmachine::step_to_angle(uint32_t step)
{
    double angle = 0;
    angle = ((double)step / 3200.0) * 360.0;
    return angle;
}

/// @brief 控制升降台复位至水平
/// @param wait true:等待复位完成, false:不等待复位完成
void PCBmachine::reset_platform(bool wait)
{
    static uint32_t MOTROR_3_4_ZERO = 10000; // 9600
    this->CtrlM3_4(MOTROR_3_4_ZERO);
    if(wait){
        wait_for_motor_movement(3, MOTROR_3_4_ZERO);
    }
}

/// @brief 等待电机运动到指定步进
/// @param id 电机编号 1-6
/// @param target_step 指定的步进角
/// @param resend_data 等待过程中是否重新发送命令
/// @param send_interval 重新发送命令的时间间隔*10ms ,当resend_data为true时生效
/// @param err 与指定步进角的误差
void PCBmachine::wait_for_motor_movement(int id, uint32_t target_step, bool resend_data, uint32_t send_interval,  uint32_t err)
{
    if(id<1 || id>6){
        std::cout << "wait_for_motor_movement: 电机编号错误" << std::endl;
    }
    typedef std::function<void(uint32_t data)> CtrlMotor;
    CtrlMotor CtrlFun = nullptr;
    if (resend_data) 
    {
        switch (id) {
            case 1:  CtrlFun  = std::bind(&PCBmachine::CtrlM1, this, std::placeholders::_1); break;
            case 2:  CtrlFun = std::bind(&PCBmachine::CtrlM2, this, std::placeholders::_1); break;
            case 3:  CtrlFun = std::bind(&PCBmachine::CtrlM3_4, this, std::placeholders::_1); break;
            case 4:  CtrlFun = std::bind(&PCBmachine::CtrlM3_4, this, std::placeholders::_1); break;
            case 5:  CtrlFun = std::bind(&PCBmachine::CtrlM5, this, std::placeholders::_1); break;
            case 6:  CtrlFun = std::bind(&PCBmachine::CtrlM6, this, std::placeholders::_1); break;
            default:break;
        }
    }

    double *ptr = &this->MotorPose.M1 + id - 1;
    int i = 0;
    if (send_interval > 10) i = send_interval - 10;
    while(this->PCBmachine_ok)
    {
        DBG("M%d实时步进: %d\n", id, angle_to_step(*ptr));
        if(abs((int)angle_to_step(*ptr) - (int)target_step) < err){
            break;
        }
        
        if (i > send_interval) {
            i = 0;
            if (resend_data)CtrlFun(target_step);
        }
        this->delay_ms(10);
        i++;
        
    }

}

/*控制电机运动的函数*/

/// @brief 电机1
/// @param step 使用绝对步进控制对应电机 , 1细分下一圈为200个步进 16细分下一圈为3200个步进
void PCBmachine::CtrlM1(uint32_t step)
{
    uint8_t buf[5];
    buf[0] = 1; // 电机编号
    U32_TO_U8ARR(&buf[1], step); // 绝对步进
    this->sendPacket(0, CMD_MOTOR_POSE, buf, 5);
}

/// @brief 电机2
/// @param step 使用绝对步进控制对应电机 , 1细分下一圈为200个步进 16细分下一圈为3200个步进
void PCBmachine::CtrlM2(uint32_t step)
{
    uint8_t buf[5];
    buf[0] = 2; // 电机编号
    U32_TO_U8ARR(&buf[1], step); // 绝对步进
    this->sendPacket(0, CMD_MOTOR_POSE, buf, 5);
}

/// @brief 电机3_4 同步运动
/// @param step 使用绝对步进控制对应电机 , 1细分下一圈为200个步进 16细分下一圈为3200个步进
void PCBmachine::CtrlM3_4(uint32_t step)
{
    uint8_t buf[5];
    buf[0] = 3; // 电机编号
    U32_TO_U8ARR(&buf[1], step); // 绝对步进
    this->sendPacket(0, CMD_MOTOR_POSE, buf, 5);
}

/// @brief 电机5
/// @param step 使用绝对步进控制对应电机 , 1细分下一圈为200个步进 16细分下一圈为3200个步进
void PCBmachine::CtrlM5(uint32_t step)
{
    uint8_t buf[5];
    buf[0] = 5; // 电机编号
    U32_TO_U8ARR(&buf[1], step); // 绝对步进
    this->sendPacket(0, CMD_MOTOR_POSE, buf, 5);
}

/// @brief 电机6
/// @param step 使用绝对步进控制对应电机 , 1细分下一圈为200个步进 16细分下一圈为3200个步进
void PCBmachine::CtrlM6(uint32_t step)
{
    uint8_t buf[5];
    buf[0] = 6; // 电机编号
    U32_TO_U8ARR(&buf[1], step); // 绝对步进
    this->sendPacket(0, CMD_MOTOR_POSE, buf, 5);
}

void PCBmachine::CtrlM7(uint8_t angle) 
{
    uint8_t buf[2] = { 7, angle };
    this->sendPacket(0, CMD_MOTOR_SERVO, buf, 2);
}

void PCBmachine::CtrlM8(uint8_t angle)
{
    uint8_t buf[2] = { 8, angle };
    this->sendPacket(0, CMD_MOTOR_SERVO, buf, 2);
}

/// @brief 回调函数, 接收到下位机对应指令时调用
/// @param data 接收到的来自下位机的数据
/// @return 
int PCBmachine::CmdTest(std::vector<uint8_t>& data)
{
    DBG("调用了CmdTest");
    return 0;
}

/// @brief 回调函数, 接收到下位机对应指令时调用
/// @param data 接收到的来自下位机的数据
/// @return 
int PCBmachine::CmdPose(std::vector<uint8_t>& data)
{
    if ( !(data.size() == 6 || data.size() == 2) ) {
        std::cout << "电机位置信息错误" << std::endl;
        return -1;
    }
    
    if (data[0] == 7 || data[0] == 8) // 舵机
    {
        double* ptr = &this->MotorPose.M1 + data[0] - 1;
        if (data.size() != 2) {
            std::cout << "电机位置信息错误(舵机数据错误)" << std::endl;
            return -1;
        }
        if (data[1] == 0) *ptr = 0;
        else  *ptr = 1;
        //DBG("更新电机%d : %.2lf \n", (int)data[0], *ptr);
        return 0;
    }
    else
    {
        if (data[0] >= 7 || data[0] == 0)
        {
            std::cout << "电机位置信息错误(未知的电机编号)" << std::endl;
            return -1;
        }

        double* ptr = &this->MotorPose.M1 + data[0] - 1;
        uint32_t mid = 0;
        mid = ((uint32_t)data[2] << 24) |
            ((uint32_t)data[3] << 16) |
            ((uint32_t)data[4] << 8) |
            ((uint32_t)data[5] << 0);

        //*ptr = mid;
        *ptr = mid * 360.0f / 65536.0f;

        if (data[1] != 0)*ptr = -(*ptr);
        //std::cout << "更新电机" << (int)data[0] << "的角度为: " << *ptr << std::endl;
        //DBG("更新电机%d : %.2lf \n", (int)data[0], *ptr);
        return 0;
    }
}

/// @brief 回调函数, 接收到下位机对应指令时调用
/// @param data 接收到的来自下位机的数据
/// @return 
int PCBmachine::CmdErr(std::vector<uint8_t>& data)
{
    DBG("调用了CmdErr\n");
    return 0;
}

void PCBmachine::delay_ms(int ms) {
    std::this_thread::sleep_for(std::chrono::milliseconds(ms));
}